#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Hubs,  S2, HTServo,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     seeker,         sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     Left,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     Left2,         tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     Right,         tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     Right2,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     blocklifter,   tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     flag,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C1_1,    Turntable,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    hook,                 tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    flipper,              tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_1,    fickleFinger,         tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_2,    gearshifter,          tServoStandard)
#pragma config(Servo,  srvo_S2_C2_3,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
//zone in which the joystick does not send values to the motors.
int deadZone = 15;
//int shootingSpeed = 100;
long turningDistance()
{
	long turDis = joystick.joy2_y2/joystick.joy2_x2;
	return turDis;
}


//This is the slow mode function that allows you to use the slow mode. Slow mode is the mode that allows you to slow down the robot.
void slowMode()
{
	//if the absolute value of joystick.joy1_y1 is greater than varible deadZone.
	if (abs(joystick.joy1_y1) > deadZone)
	{
		//The left motor's value is equal to the value joystick.joy1_y1 divided by 4.
		motor[Right] = -joystick.joy1_y1 / 4;
		motor[Right2] = joystick.joy1_y1 / 4;
	}
	//if the if statement above is not entered then
	else
	{
		//The left motor's value equals 0.
		motor[Right] = 0;
		motor[Right2] = 0;
	}
	//This is the same code as above but for the right side
	if (abs(joystick.joy1_y2) > deadZone)
	{
		motor[Left] = -joystick.joy1_y2 / 4;
		motor[Left2] = joystick.joy1_y2 / 4;
	}
	else
	{
		motor[Left] = 0;
		motor[Left2] = 0;
	}
}

//This is the secound task that splits the two joysticks into two tasks to help with efficency
task Joystick2()
{
	while (true)
	{
		//This gets the value of the joysticks from the robot
		getJoystickSettings(joystick);

		/*if (joy2Btn(2) == 1)
		{
			motor[blocklifter] = 100;
		}
		else if (joy2Btn(3) == 1)
		{
			motor[blocklifter] = -100;
		}
		else
		{
			motor[blocklifter] = 0;
		}*/

		if (joy2Btn(4) == 1)
		{
			motor[flag] = 100;
		}
		else if (joy2Btn(1) == 1)
		{
			motor[flag] = -100;
		}
		else
		{
			motor[flag] = 0;
		}


		if (joystick.joy2_TopHat == 2 && ServoValue[Turntable] < 250)
		{
			servo[Turntable] = 255;
		}
		else if (joystick.joy2_TopHat == 6 && ServoValue[Turntable] > 0)
		{
			servo[Turntable] = 10;
		}
		else
		{
			servo[Turntable] = 128;
		}

		if(joystick.joy2_y1 > deadZone)
		{
			servo[hook] = joystick.joy2_y1 + 128;
		}
		else if(joystick.joy2_y1 < deadZone)
		{
			servo[hook] = joystick.joy2_y1 + 128;
		}
		else
		{
			servo[hook] = 10;
		}

		if (joy2Btn(7) == 1)
		{
			servo[fickleFinger] = 255;
		}
		else if (joy2Btn(8) == 1)
		{
			servo[fickleFinger] = 0;
		}
		else
		{
			servo[fickleFinger] = 128;
		}

		if (joystick.joy2_TopHat == 0)
		{
			servo[flipper] = 128;
		}
		else if (joystick.joy2_TopHat == 4)
		{
			servo[flipper] = 0;
		}
	}
}


task main()
{
servo[hook] = 10;
servo[gearshifter] = 0;
servo[Turntable] = 255;
servo[fickleFinger] = 128;
servo[flipper] = 0;
waitForStart();
StartTask(Joystick2);
while (true)
{

	getJoystickSettings(joystick);
	if (joy1Btn(8) == 1)
	{
		slowMode();
	}
	else
	{
		if (abs(joystick.joy1_y1) > deadZone)
		{
			motor[Right] = -joystick.joy1_y1;
			motor[Right2] = joystick.joy1_y1;
		}
		else
		{
			motor[Right] = 0;
			motor[Right2] = 0;
		}

		if (abs(joystick.joy1_y2) > deadZone)
		{
			motor[Left] = -joystick.joy1_y2;
			motor[Left2] = -joystick.joy1_y2;
		}
		else
		{
			motor[Left] = 0;
			motor[Left2] = 0;
		}

		if (joystick.joy1_TopHat == 2)
		{
			servo[gearshifter] = 128;
		}
		else
		{
			servo[gearshifter] = 0;
		}
	}
}
}
